#include "sv2_gimbal_sms.h"

namespace sv2
{
  void gimbalsetZoom(double x)
  {
    gimbal->setZoom(x);
    gimbal_state_msg["is_zoom"] = 1;
  }

  void GimbalCppNode::run()
  {
    while (this->is_running())
    {
      // 主线程阻塞，等待队里中出现数据
      //std::unique_lock<std::mutex> lock(this->_ctl_queue_mtx);
      //this->_gimbal_ctl.wait(lock, [this]
      //                       { return !this->_ctl_queue.empty(); });

      //nlohmann::json json_ctl = this->_ctl_queue.front();
      //this->_ctl_queue.pop();
      nlohmann::json json_ctl;

      // Gimbal control for tracking
      if (this->ctl_mode == 1)
      {
        gimbal->setAngleRateEuler(this->angle_euler_rate_cmd[0], this->angle_euler_rate_cmd[1], this->angle_euler_rate_cmd[2]);
        gimbal_state_msg["move_mode"] = 3;
      }
      else if (this->ctl_mode == 2)
      {
        gimbal->setAngleEuler(this->angle_euler_cmd[0], this->angle_euler_cmd[1], this->angle_euler_cmd[2]);
        gimbal_state_msg["move_mode"] = 0;
      }

      // Automatic Zoom, only for GX40
      if (this->zoom > 1)
      {
        gimbalsetZoom(this->zoom);
      }
      else
      {
        gimbalsetZoom(1);
      }
      _gimbal_state_pub.publish(gimbal_state_msg);
      std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
  }

}
